#include <Servo.h>
Servo myservo;
const int roodHoofdWeg = 2;
const int geelHoofdWeg = 3;
const int groenHoofdWeg = 4;
const int roodZijWeg = 5;
const int geelZijWeg = 6;
const int groenZijWeg = 7;
const int speaker = 10;
const int servo = 11;
const int overwegLicht = 12;
char nextchar = 'a';
String message;
int traincounter = 0;
int lightdelay = 700;
bool overwegactief = false;
int servocounter = 0;

void setup()
{
  Serial.begin(9600);
  pinMode(roodHoofdWeg ,OUTPUT);
  pinMode(geelHoofdWeg, OUTPUT);
  pinMode(groenHoofdWeg, OUTPUT);
  pinMode(roodZijWeg, OUTPUT);
  pinMode(geelZijWeg, OUTPUT);
  pinMode(groenZijWeg, OUTPUT);
  pinMode(speaker, OUTPUT);
  pinMode(servo, OUTPUT);
  pinMode(overwegLicht,OUTPUT);
  pinMode(A1, INPUT);
  pinMode(A5, INPUT);
  myservo.attach(11);
}

void loop()
{
  //state: [IDLE]
  // in deze state blijft hij wachten op een # van de seriele poort.
  nextchar = Serial.read();
  // een # betekent dat er een bericht binnenkomt, anders blijft hij in state [IDLE]
  if(nextchar == '#')
  {
    message = startreadcommand(); // naar state [COMMANDO-LEZEN]
    processmessage(message);      // naar state [COMMANDO-UITVOEREN]
  }

  railroadSystem(); //zie functie
}

String startreadcommand()
{
  //dumps an array of characters into a string until % is read
  String incoming = "#";
  while(nextchar != '%')
  {
    nextchar = readNextCharacterFromSerial();
    incoming = incoming + nextchar;    
  }
  return incoming;
}

char readNextCharacterFromSerial()
{ 
  //keeps reading until character is received
  int value = -1;
  do
  {
    value = Serial.read();
  } 
  while(value == -1);
  return (char) value;
}
void processmessage(String message)
{
  if(message == "#MAINROADRED%")
  {
    digitalWrite(roodHoofdWeg, HIGH);
    digitalWrite(geelHoofdWeg, LOW);
    digitalWrite(groenHoofdWeg, LOW);
  }
  else if(message == "#MAINROADYELLOW%")
  {
    digitalWrite(roodHoofdWeg, LOW);
    digitalWrite(geelHoofdWeg, HIGH);
    digitalWrite(groenHoofdWeg, LOW);
  }
  else if(message == "#MAINROADGREEN%")
  {
    digitalWrite(roodHoofdWeg, LOW);
    digitalWrite(geelHoofdWeg, LOW);
    digitalWrite(groenHoofdWeg, HIGH);
  }
  else if(message == "#SIDEROADRED%")
  {
    digitalWrite(roodZijWeg, HIGH);
    digitalWrite(geelZijWeg, LOW);
    digitalWrite(groenZijWeg, LOW);
  }
  else if(message == "#SIDEROADYELLOW%")
  {
    digitalWrite(roodZijWeg, LOW);
    digitalWrite(geelZijWeg, HIGH);
    digitalWrite(groenZijWeg, LOW);
  }
  else if(message == "#SIDEROADGREEN%")
  {
    digitalWrite(roodZijWeg, LOW);
    digitalWrite(geelZijWeg, LOW);
    digitalWrite(groenZijWeg, HIGH);
  }
}

void railroadSystem()
{
  if(!overwegactief)
  {
    if(analogRead(5) >=600)
    {
      Serial.println("#TRAINCOMING%");
      traincounter = 700;
      servocounter = 0;
      overwegactief = true;
    }
  }
  if(traincounter > 0)
  {
      digitalWrite(overwegLicht, HIGH);
    //servos
    if(servocounter < 75)
    {
      myservo.write(80+servocounter);
    }
    if(servocounter > 625)
    {
      myservo.write(780-servocounter); 
    }
    delay(10);
    traincounter --;
    servocounter++;
  }
    else
    {
      overwegactief = false;
      myservo.write(80);
      digitalWrite(overwegLicht, LOW);
    }

}

